import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription

from launch.substitutions import LaunchConfiguration, Command, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue



def generate_launch_description():
    # 获取功能包的路径
    pkg_project_fishbot = get_package_share_directory("fishbot_description")

    use_sim_time = LaunchConfiguration('use_sim_time', default='true')
    x_pose = LaunchConfiguration('x_pose', default='1.0')
    y_pose = LaunchConfiguration('y_pose', default='1.0')
    
    # 世界模型文件的路径
    world_path = PathJoinSubstitution([
        FindPackageShare('fishbot_description'),
        'world',
        'custom_room.world'
    ])
    
    # 启动gazebo仿真
    gz_sim = IncludeLaunchDescription(
        PathJoinSubstitution([
            FindPackageShare('ros_gz_sim'),
            'launch',
            'gz_sim.launch.py'
        ]),
        # -r 启动后立即仿真
        launch_arguments={
            'gz_args': ['-r ', world_path]
        }.items()
    )
    
    # 将xacro文件转化成urdf，并作为字符串放到变量robot_description中
    robot_description = ParameterValue(
        Command(['xacro ', PathJoinSubstitution([
            FindPackageShare('fishbot_description'),
            'urdf',
            'fishbot',
            'fishbot.urdf.xacro'
        ])]), value_type=str
    )
    
  	# 机器人状态发布节点
    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{
            'use_sim_time': use_sim_time,
            'robot_description': robot_description,
            'publish_frequency': 50.0,
        }]
    )
    
    # 通过机器人描述话题，生成机器人模型
    spawn_robot = Node(
        package='ros_gz_sim',
        executable='create',
        arguments=[
            '-name', 'fishbot',
            '-topic', 'robot_description',
            '-x', x_pose,
            '-y', y_pose,
            '-z', '0.01'
        ],
        output='screen',
    )
    
    # ros和gazebo通信桥，参数文件路径
    bridge_params = os.path.join(
        pkg_project_fishbot,
        'param',
        'fishbot_bridge.yaml'
    )

    # 启动通信桥
    start_gazebo_ros_bridge_cmd = Node(
        package='ros_gz_bridge',
        executable='parameter_bridge',
        arguments=[
            '--ros-args',
            '-p',
            f'config_file:={bridge_params}',
        ],
        output='screen',
    )
    
    # 在RViz中可视化
    rviz = Node(
        package='rviz2',
        executable='rviz2',
        arguments=['-d', PathJoinSubstitution([
            FindPackageShare('fishbot_description'),
            'config',
            'display_robot_model.rviz'
        ])]
    )
    
    ld = LaunchDescription()

    # 将命令添加到启动描述中
    ld.add_action(gz_sim)
    ld.add_action(robot_state_publisher)
    ld.add_action(spawn_robot)
    ld.add_action(start_gazebo_ros_bridge_cmd)
    ld.add_action(rviz)

    return ld
